/**
 * Implement intersection between primitives. 
 * @author Songgang Xu (sxu at tamu dot edu)
 * @file intersect.h
 */

#ifndef _XMESH_INTERSECT_3D_H
#define _XMESH_INTERSECT_3D_H

#include "predicates.h"
#include "intersect_2d.h"
#include "predicate_3d.h"

/**
 * Calculates the intersection of two 3D lines. 
 * The method is as follows. 

 L1(t1) = p1 + d1*t1
 L2(t2) = p2 + d2*t2

 The solution is:
 t1 = Determinant{(p2-p1),d2,d1 X d2} / ||d1 X d2||^2
 and
 t2 = Determinant{(p2-p1),d1,d1 X d2} / ||d1 X d2||^2

 If the lines are parallel, the denominator ||d1 X d2||^2 is 0.
 If the lines do not intersect, t1 and t2 mark the points of closest approach on each line.
*/
template <typename Value>
inline bool intersect (const Line <Value> & _line1, const Line <Value> & _line2, IntersectResult <Value> & _intr)
{
	//Points
	const Vector <Value> & p1 = _line1.get_point() ;
	const Vector <Value> & p2 = _line2.get_point() ; 

	const Vector <Value> & d1 = _line1.get_vector() ;
	const Vector <Value> & d2 = _line2.get_vector() ; 

	Value v = (d1^d2).square_module(); 
	if (abs(v) < TINY ) 
	{
		if (((p1-p2)^d1 ).square_module() < TINY )
		{
			_intr.res_type = LINE ;
			_intr.line = _line1 ;
			return true; 
		}
		return false; 
	}

	DenseMatrix <Value> matrix (3, 3);
	matrix[0] = p2 - p1; matrix[1] = d2 ; matrix[2] = d1 ^ d2 ;

	Value t1 = matrix.det3()  ;
	Vector<Value> intr1 = p1 * v + d1 * t1 ;

	matrix[1] = d1;
	Value t2 = matrix.det3()  ; 
	Vector<Value> intr2 = p2 * v + d2 * t2 ;

	/** The robustness must be committed here. */
	if ((intr1 - intr2).square_module() < Value(TINY) )
	{
		_intr.point = intr1 / v;
		_intr.res_type = POINT ;
		return true; 
	}
	return false ;
}


/**
 *  Calculates the intersection of two 3D segments. 
 */
template <typename Value>
inline bool intersect (const Segment <Value> & _seg1, const Segment <Value> & _seg2, IntersectResult <Value> & _intr)
{

	bool rvalue = intersect(Line <Value> (_seg1.p0, _seg1.p1 - _seg1.p0), Line <Value> (_seg2.p0, _seg2.p1 - _seg2.p0), _intr );
//    Value test = (_seg1.p1 - _seg1.p0 ) * (_seg2.p1 - _seg2.p0);

	if (rvalue == true) 
	{
		if (_intr.pos == INTERSECT)
		{
			Value d1 = ( _seg1.p0 - _intr.point ) * (_seg1.p1 - _intr.point) ;
			Value d2 = ( _seg2.p0 - _intr.point ) * (_seg2.p1 - _intr.point) ;
			if (d1 <= 0 && d2 <=0) return true; 
			// Guess if the intersection point are the ends. 
			if ((_intr.point - _seg1.p0 ).square_module() < TINY)
			{
				_intr.point = _seg1.p0; 
				return true; 
			}
			if ((_intr.point - _seg1.p1 ).square_module() < TINY)
			{
				_intr.point = _seg1.p1; 
				return true; 
			}
			//otherwise. 
  			_intr.pos = NONINTERSECT ;
   			return false; 
		}
		else if ( _intr.pos == OVERLAP)
		{
			/*We know two segments are co-line.*/
			Segment <Value> s1 = _seg1 ;
			Segment <Value> s2 = _seg2 ;
			s1.order (); s2.order(); 

			if (s1.p0 < s2.p0)
			{
				//s1p0, s1p1 , s2p0, s2p1
				if (s1.p1 < s2.p0) {_intr.pos = NONINTERSECT; return false;}
				//s1p0, s1p1 -- s2p0, s2p1
				if (s1.p1 == s2.p0 ) { _intr.res_type = POINT; _intr.point = s1.p1 ; return true; }
				//s1p0, s2p0, s1p1 s2p1
				if (s1.p1 > s2.p0 && s1.p1 <= s2.p1) { _intr.res_type = SEGMENT; _intr.segment = Segment<Value>(s2.p0, s1.p1) ; return true; }
				//s1p0, s2p0, s2p1, s1p1
				if (s1.p1 > s2.p1) { _intr.res_type = SEGMENT; _intr.segment = s2 ; return true; }
			}
			else if (s2.p0 <= s1.p0)
			{
				//s2p0, s2p1 , s1p0, s1p1
				if (s2.p1 < s1.p0) {_intr.pos = NONINTERSECT; return false;}
				//s2p0, s2p1 -- s1p0, s1p1
				if (s2.p1 == s1.p0 ) { _intr.res_type = POINT; _intr.point = s1.p0 ; return true; }
				//s2p0, s1p0, s2p1 s1p1
				if (s2.p1 > s1.p0 && s2.p1 <= s1.p1) { _intr.res_type = SEGMENT; _intr.segment = Segment<Value>(s1.p0, s2.p1) ; return true; }
				//s2p0, s1p0, s1p1, s2p1
				if (s2.p1 > s1.p1) { _intr.res_type = SEGMENT; _intr.segment = s1 ; return true; }				
			}
			else
			{
				_intr.pos = NONINTERSECT; 
				return false;
			}
		}
	}
	return rvalue ;
}


/**
 * Calculates the intersection between a plane and a segment. 
 * The idea is we map the segment to the plane. Compute the intersection between the segment and its image on the plane. 
 */
template <typename Value>
inline bool intersect (const Plane <Value> & _plane, const Segment <Value> & _segment, IntersectResult <Value> & _intr)
{
/*
  Value dist0 = _plane.dist (_segment.p0); 
  Value dist1 = _plane.dist (_segment.p1); 

  if (dist0 == Value(0) and dist1 == Value(0)) throw ("The segment is embeded in the plan. More than one intersection. ");

  if (dist0 * dist1 > Value(0))
  {
  return false ;
  }
*/
	Segment <Value> proj;
	proj.p0 = _plane.project(_segment.p0);
	proj.p1 = _plane.project(_segment.p1);

	//Points are on the same side. 
	if ((proj.p0 - _segment.p0 ) * (proj.p1 - _segment.p1) > Value(0))
	{
		_intr.pos = NONINTERSECT ;
		return false; 
	}
	// Segment is perpendicular to the plane
	if (proj.square_length() < TINY)
	{
		_intr.pos = INTERSECT ;
		_intr.res_type = POINT ;
		_intr.point = proj.p0 ;
		return true; 
	}
	//Points maybe on the different side.
	return intersect (_segment, proj, _intr);
}

/**
 * Calculates the intersection between a convex polygon and a segment. 
 - Compute the segment intersect the plane. 
 - Decide if the intersection is inside the polygon.
*/
template <typename Value>
inline bool intersect (const Polygon <Value> & _polygon, const Segment <Value> & _segment, IntersectResult <Value> & _intr)
{
	Plane <Value> plane = _polygon.get_plane();
	bool rvalue = intersect(plane, _segment, _intr);

	if (rvalue == false) 
	{
		return false; 
	}
	if (_intr.pos == INTERSECT && _intr.res_type == POINT)
	{
		if (_polygon.is_on(_intr.point) && _segment.is_on(_intr.point))
		{			
			_intr.pos = INTERSECT ;
			return true; 
		}
		else
		{
			_intr.pos = NONINTERSECT; 
			return false; 
		}
	}
	// Co-plane segment and polygon. The idea is 
	// First, if the two ends of the segment is in the polygon.
	// Second, check the intersection of segments of polygon with the segment.
	if (_intr.pos == OVERLAP && _intr.res_type == SEGMENT)
	{
		bool bp0 = _polygon.is_on(_segment.p0);
		bool bp1 = _polygon.is_on(_segment.p1);
		if (bp0 && bp1)
		{
			//the segment is inside of the polygon.
			return  true; 
		}
		if (bp0 && !bp1 ) 
		{
			unsigned size = _polygon.size();
			for (unsigned i = 0; i < size ; ++i)
			{
				Segment<Value> seg (_polygon[i], _polygon[(i+1)%size]);
				if (seg.is_on(_segment.p0))
				{
					_intr.pos = INTERSECT ;
					_intr.res_type = POINT ;
					_intr.point = _segment.p0; 
					return true; 
				}
				else
				{
					bool rvalue = intersect(_segment, seg, _intr);
					if (rvalue == false) return false;
					if (_intr.pos == OVERLAP)
					{
						return true;
					}
					if (_intr.pos == INTERSECT && _intr.res_type == POINT)
					{
						_intr.pos = INTERSECT ;
						_intr.res_type = SEGMENT ;
						_intr.segment = Segment<Value> (_segment.p0, _intr.point);
					}
					return rvalue; 
				}
			}
		}
		if (!bp0 && bp1)
		{
			unsigned size = _polygon.size();
			for (unsigned i = 0; i < size ; ++i)
			{
				Segment<Value> seg (_polygon[i], _polygon[(i+1)%size]);
				if (seg.is_on(_segment.p1))
				{
					_intr.pos = INTERSECT ;
					_intr.res_type = POINT ;
					_intr.point = _segment.p1; 
					return true; 
				}
				else
				{
					bool rvalue = intersect(_segment, seg, _intr);
					if (rvalue == false) return false;
					if (_intr.pos == OVERLAP)
					{
						return true; 
					}
					if (_intr.pos == INTERSECT && _intr.res_type == POINT)
					{
						_intr.pos = INTERSECT ;
						_intr.res_type = SEGMENT ;
						_intr.segment = Segment<Value> (_segment.p1, _intr.point);
					}
					return rvalue; 
				}
			}
		}
		if (!bp0 && !bp1)
		{
			//std::cout<<" The two ends are not in the convex polygon"<<std::endl;
			unsigned size = _polygon.size();
			std::vector<Vector<Value> > inter_point; 
			for (unsigned i = 0; i < size ; ++i)
			{
				Segment<Value> seg (_polygon[i], _polygon[(i+1)%size]);
				bool rvalue = intersect (seg, _segment, _intr ) ;
				if (rvalue == false) return false;
				if (_intr.res_type == SEGMENT)
				{
					return true; 
				}
				if (_intr.pos == INTERSECT && _intr.res_type == POINT)
				{
					inter_point.push_back(_intr.point);
				}
			}
			if (inter_point.size() == 2)
			{
				_intr.pos = OVERLAP ;
				_intr.res_type = SEGMENT; 
				_intr.segment = Segment<Value> (inter_point[0], inter_point[1]);
				return true; 
			}
			else
			{
				std::cout<<"NON CONVEX POLYGON"<<inter_point.size()<<std::endl;
				return false;
			}

		}
	}


	
	std::cout<<_intr.pos<<_intr.res_type<<std::endl;
	throw ("Coplane Segment - polygon hasn't been done!");
}

/**
 * Calculates the intersection between a plane and a convex polygon. 
 * Remark. We assume that the polygon is convex. I want to write an non-convex polygon intersection later, because it is a little hard.
 The algorithm is as follows.
 - We check if some edge is on the plane. If so, return this edge. 
 - Otherwise, find the intersection point of each edge with plane and store them.
 - connect these point and remove the degeneracy edge. Actually we have at most one edge. 
*/
template <typename Value>
inline bool intersect (const Plane <Value> & _plane, const Polygon <Value> & _polygon, IntersectResult <Value> & _intr)
{
/*
	std::vector<Vector<Value> > segmentPts; 
	unsigned size = _polygon.get_nb_vertices();
	unsigned overlap_edge_count = 0;
	unsigned overlap_edge_idx = 0;
	// check if overlap with the plane or have one edge on the plane
	for (unsigned i = 0; i < size; ++i)
	{
		Segment<Value> seg (_polygon[i], _polygon[(i+1)%size]);
		intersect (_plane, seg, _intr); 
		if (_intr.pos == OVERLAP) 
		{
			overlap_edge_count ++ ;
			overlap_edge_idx = i;
		}
	}
	if (overlap_edge_count >=2)
	{
		_intr.pos = OVERLAP;
		_intr.res_type = POLYGON ;
		_intr.polygon = _polygon; 
		return true; 
	}
	else if  (overlap_edge_count == 1)
	{
		_intr.pos = INTERSECT;
		_intr.res_type = SEGMENT ;
		_intr.segment = Segment<Value> (_polygon[overlap_edge_idx], _polygon[(overlap_edge_idx+1)%size]);
	}

	// find all the possible segment. 
	for (unsigned i = 0; i < size; ++i)
	{
		Segment<Value> seg (_polygon[i], _polygon[(i+1)%size]);
		intersect (_plane, seg, _intr); 
		if (_intr.pos == INTERSECT) segmentPts.push_back (_intr.point);
	}

	size = segmentPts.size();
	if (size == 0) return false; 

	//Find the longest segment. remove the degeneracy. 
	unsigned index = 0;
	Value length = 0;
	for (unsigned i = 0; i < size; ++i)
	{
		Value squareModule = (segmentPts[i] - segmentPts[(i+1)%size] ).square_module();
		if (  squareModule> length)
		{
			length = squareModule ;
			index = i ;
		}
	}
	_intr.segment = Segment<Value> (segmentPts[index] , segmentPts[(index+1)%size] );


	if (_intr.segment.square_length() < TINY)
	{
		_intr.point = _intr.segment.p0; 
		_intr.pos = INTERSECT ;
		_intr.res_type = POINT ;
		return true; 
	}
	else
	{
		_intr.pos = INTERSECT ;
		_intr.res_type = SEGMENT ;
		return true; 
	}
	return true; 
*/
	throw ("Incomplete function.");
	return false;
}

template <typename Value>
inline Vector<Value> intersect_seg_triangle (const Vector<Value> & p1, const Vector<Value> & r1, 
											 const Vector<Value> & p2, const Vector<Value> & q2, const Vector<Value> & r2)
{
	Vector<Value> v1, v2, N2; 
	Value alpha; 
	N2  = (q2 - p2) ^ (r2 - p2) ;
	v1  = p1 - p2;
	v2  = p1 - r1;
	alpha = (v1*N2) / (v2*N2);
	v1  = v2 * alpha ;
	return ( p1 - v1 );
}

/**
 * Intersection ray triangle. 
 */
template <typename Value>
bool intersect_ray_triangle (const Ray<Value> & _ray, const Polygon<Value> & _pol, IntersectResult<Value> & _intr)
{
	if (_pol.size() != 3) throw ("Ray - triangle perdicate is not called with a triangle. ");
	Value orig[3], dir[3];
	Value vert0[3], vert1[3],  vert2[3]; 
	Value t, u, v ;
	Vector<Value> p, d ;
	p = _ray.get_point();
	d = _ray.get_direction ();

	for (int i = 0; i < 3; ++i)
	{
		orig[i] = p[i];
		dir[i]  = d[i];
		vert0[i] = _pol[0][i];
		vert1[i] = _pol[1][i];
		vert2[i] = _pol[2][i];
	}
	bool resValue = _intersect_ray_tri(orig, dir, vert0, vert1, vert2, & t, & u, &v);
	if (resValue == false) return false; 

	_intr.res_type = POINT; 
	//Vector<Value> pi  = _pol[0] * (Value(1.0f) - u - v) + _pol[1] * u + _pol[2] * v; 
	//std::cout<<"t value"<<t<<std::endl;
	_intr.point = p + t * d ; /// d.module(); 
	//std::cout<<pi<<_intr.point<<std::endl;
	return true; 
}

/**
 * Calculates the intersection between two Convex Polygons. 
 */
template <typename Value>
bool intersect_polygon_polygon (const Polygon <Value> & _pol1, const Polygon <Value> & _pol2, IntersectResult<Value> & _intr)
{
	Vector<Value> normal1 = _pol1.calculate_normal();
	Vector<Value> normal2 = _pol2.calculate_normal();

	unsigned size1 = _pol1.size();
	unsigned size2 = _pol2.size(); 
	unsigned u0, u1, u2, u3;
	u0 =  u1 =  u2 = u3 = size1; 

	unsigned coplanar = 0;

	unsigned i = 0, j = size1 - 1; 
	for (; i < size1; j = i++)
	{
		Value d0 = (_pol1[j] - _pol2[0]) * normal2 ; // Predicate
		Value d1 = (_pol1[i] - _pol2[0]) * normal2 ; // Predicate
		if (d0 * d1 <= Value(0))
		{
			if (d0 >= Value(0) && d1 < Value(0)) { u0 = j ; u1 = i; }
			else if (d0 > Value(0) && d1 <= Value(0)) { u0 = j ; u1 = i; }
			else if (d1 >= Value(0) && d0 < Value(0)){ u3 = i ; u2 = j; }
			else if (d1 > Value(0) && d0 <= Value(0)){ u3 = i ; u2 = j; }
			else
			{
				coplanar++;
			}
		}
	}
	if (coplanar >= 3) 
	{	
		return intersect_polygon_polygon_coplanar ( _pol1, _pol2, _intr);
		//return (predicate_polygon_polygon_convex_coplanar(_pol1, _pol2));
	}
	if ( u0 == size1 || u2 == size1 ) return false;  
	
	unsigned v0, v1, v2, v3 ;
	v0 = v1 = v2 = v3 = size2; 
 
	for (i = 0, j = size2 - 1; i < size2; j = i++)
	{
		Value d0 = ( _pol2[j] - _pol1[0] ) * normal1 ; // Predicate
		Value d1 = ( _pol2[i] - _pol1[0] ) * normal1 ; // Predicate
		if (d0 * d1 <= Value(0))
		{
			if (d0 >= Value(0) && d1 < Value(0)) { v0 = j ; v1 = i; }
			else if (d0 > Value(0) && d1 <= Value(0)) { v0 = j ; v1 = i; }
			else if (d1 >= Value(0) && d0 < Value(0)){ v3 = i ; v2 = j; }
			else if (d1 > Value(0) && d0 <= Value(0)){ v3 = i ; v2 = j; }
		}
	}
	if (v0 == size2 || v2 == size2 ) return false; 

	Value d0 = orient_3d (_pol1[u0], _pol1[u1], _pol2[v0], _pol2[v1]);
	Value d1 = orient_3d (_pol1[u2], _pol1[u3], _pol2[v3], _pol2[v2]);
	
	if (!(d0 <= 0 && d1 <= 0)) return false;

	Value d2 = orient_3d (_pol1[u0], _pol1[u1], _pol2[v3], _pol2[v2]);
	Value d3 = orient_3d (_pol1[u2], _pol1[u3], _pol2[v0], _pol2[v1]);

	// Construct the intersection here. 
	Vector<Value> source, target; 
	unsigned u, v; 
	if (d2 < Value(0))
	{
		if (u1 == u2) u = u3; else u = u2; 
		source = intersect_seg_triangle (_pol2[v3], _pol2[v2], _pol1[u0], _pol1[u1], _pol1[u]); 
	}
	else 
	{
		if (v1 == v2) v = v3; else v = v2;
		source = intersect_seg_triangle (_pol1[u0], _pol1[u1], _pol2[v0], _pol2[v1], _pol2[v]);
	}

	// The intersection part is a point. 

	if (d0 == 0 || d1 == 0)
	{
		_intr.res_type = POINT ;
		_intr.point = source ;
		return true; 
	}

	if (d3 < Value(0))
	{
		if (u1 == u2) u = u3;  else u = u2; 
		target = intersect_seg_triangle (_pol2[v0], _pol2[v1], _pol1[u0], _pol1[u1], _pol1[u]);
	}
	else
	{
		if (v1 == v2) v = v3; else v = v2; 
		target = intersect_seg_triangle (_pol1[u2], _pol1[u3], _pol2[v0], _pol2[v1], _pol2[v]);
	}

	// The intersection part is a segment. 
	_intr.res_type = SEGMENT ;
	_intr.segment = Segment<Value>(source, target);

	return true; 
}

/**
 * This funciton handle the co-planar polygon intersect polygon. 
 - The idea is we first map two polygon to plane. 
 - Compute the intersection polygon.
 - Reverse the mapping via the plane equation of which the polygon falls in. 
 */
template <typename Value>
bool intersect_polygon_polygon_coplanar (const Polygon <Value> & _pol1, const Polygon <Value> & _pol2, IntersectResult<Value> & _intr)
{
	Vector<Value> normal = _pol1.calculate_normal();
	unsigned x, y; 
	//unsigned coord = 
    select_map_axis(normal, x, y);

	Polygon <Value> _pol1_2d, _pol2_2d;
	for (unsigned i = 0; i < _pol1.size(); ++i)
	{
		_pol1_2d.append_vertex (Vector<Value> (_pol1[i][x], _pol1[i][y]) );
	}

	for (unsigned i = 0; i < _pol2.size(); ++i)
	{
		_pol2_2d.append_vertex (Vector<Value> (_pol2[i][x], _pol2[i][y]) );
	}

	//Guess orientation
	int orient_pol1 = orient_2d (_pol1_2d[0], _pol1_2d[1], _pol1_2d[2]);
	int orient_pol2 = orient_2d (_pol2_2d[0], _pol2_2d[1], _pol2_2d[2]);

	// Two polygon are oriented oppositely.
	if (orient_pol1 * orient_pol2 < 0) return false; 

	unsigned size1 = _pol1_2d.size() ;
	unsigned size2 = _pol2_2d.size() ;
	if (orient_pol1 < 0)
	{
		//_reverse _pol1_2d and _pol2_2d
		for (unsigned i = 0; i <  size1 / 2; ++i)
		{
			swap (_pol1_2d[i], _pol1_2d[size1 - i - 1]) ;
		}

		for (unsigned i = 0; i < size2 /2 ; ++i)
		{
			swap (_pol2_2d[i], _pol2_2d[size1 - i - 1]) ;
		}		
	}


	IntersectResult<Value> inter_2d; 
	bool rvalue = intersect_conv_poly_poly_2d (_pol1_2d, _pol2_2d, inter_2d);
	if (rvalue == false) return false; 


	Polygon<Value> p = _pol1; 
	p.set_normal(p.calculate_normal());
	Plane<Value> plane = p.get_plane(); 
	switch (inter_2d.res_type)
	{
	case POINT: 
		_intr.res_type = POINT ;
		map_2D_point_to_3D_point(x, y, plane, inter_2d.point, _intr.point); 
		return true; 
		break;
	case SEGMENT :
		_intr.res_type = SEGMENT ;
		map_2D_point_to_3D_point(x, y, plane, inter_2d.segment.p0 , _intr.segment.p0); 
		map_2D_point_to_3D_point(x, y, plane, inter_2d.segment.p1 , _intr.segment.p1);
		return true; 
		break;
	case POLYGON :
		_intr.res_type = POLYGON ;
		if (orient_pol1 > 0)
		{
			for (unsigned i = 0; i < inter_2d.polygon.size(); ++i)
			{
				Vector<Value> p; 
				map_2D_point_to_3D_point(x, y, plane, inter_2d.polygon[i], p);
				_intr.polygon.append_vertex(p);
			}
		}
		else
		{
			for (int i = (int)inter_2d.polygon.size() - 1; i >=0 ; --i)
			{
				Vector<Value> p; 
				map_2D_point_to_3D_point(x, y, plane, inter_2d.polygon[i], p);
				_intr.polygon.append_vertex(p);
			}
		}
		return true; 
		break;
	default:
		return false;
	}
	return false; 
}

template <typename Value>
bool is_on_edge_3d (const Polygon<Value> & pol, const Vector<Value> & point1, unsigned & idx)
{
	//std::cout<<"Polygon "<<pol<<std::endl;
	//std::cout<<"Point "<<point1<<std::endl;
	const unsigned size = pol.size();
	for (unsigned i = 0; i < size; ++i)
	{
		const Vector<Value> & p1 = pol[i]; 
		const Vector<Value> & p2 = pol[(i + 1) % size]; 

		//std::cout<<"P1 P2 "<<p1<<p2<<std::endl;

		Vector<Value> d1 = p1 - point1 ;
		Vector<Value> d2 = point1 - p2;

		d1.normalize();
		d2.normalize(); 
		//std::cout<<"ABCD "<<a<<' '<<b<<' '<<c<<' '<<d<<std::endl;

		if (  abs (d1 * d2 - Value(1)) <= tiny_value) { idx = i ; return true;} 
	}
	return false; 
}

template <typename Value>
bool is_on_edge (const Polygon<Value> & pol, const Vector<Value> & point1, unsigned & idx)
{
	//std::cout<<"Polygon "<<pol<<std::endl;
	//std::cout<<"Point "<<point1<<std::endl;
	const unsigned size = pol.size();
	for (unsigned i = 0; i < size; ++i)
	{
		const Vector<Value> & p1 = pol[i]; 
		const Vector<Value> & p2 = pol[(i + 1) % size]; 

		//std::cout<<"P1 P2 "<<p1<<p2<<std::endl;

		const Value & a = p1[0] - point1[0];
		const Value & b = p1[1] - point1[1];
		const Value & c = p2[0] - point1[0];
		const Value & d = p2[1] - point1[1];

		//std::cout<<"ABCD "<<a<<' '<<b<<' '<<c<<' '<<d<<std::endl;

		//std::cout<< abs (a*d - b *c)<<std::endl;
		if (  abs (a * d - b * c ) <= tiny_value) { idx = i ; return true;} 
	}
	return false; 
}
template <typename Value>
bool is_vertices (const Polygon<Value> & pol, const Vector<Value> & point1)
{
	const unsigned size = pol.size(); 
	for (unsigned i = 0; i < size; ++i)
	{
		if ((pol[i] - point1).square_module() < tiny_value) return true; 
	}
	return false; 
}

template <typename Value>
void cut_lines (const Polygon<Value> orig_pol, const Polygon<Value> & pol, std::vector<std::vector<Vector<Value> > > & lines, std::vector<pair <unsigned, unsigned > > & he_pair_idx)
{
	std::cout<<"Original pol "<<orig_pol<<std::endl;
	std::cout<<"Polygon      "<<pol<<std::endl;
	const unsigned size = pol.size();

	// Find all the cords segments. 
	for (unsigned i = 0; i < size; ++i)
	{
		unsigned idx1, idx2 ;
		if (is_on_edge (orig_pol, pol[i], idx1) && is_on_edge (orig_pol, pol[(i+1)%size], idx2)
			&& ! is_vertices (orig_pol, pol[i]) && ! is_vertices (orig_pol, pol[(i+1)%size]))
		{
			if (idx1 != idx2)
			{
				std::cout<<"CutLine\n";
				std::vector<Vector <Value> > cut_line ;
				cut_line.push_back (pol[i]); 			
				cut_line.push_back (pol[(i+1) % size]); 
				std::cout<<"CL Point"<<pol[(i)%size]<<std::endl;
				std::cout<<"CL Point"<<pol[(i + 1)%size]<<std::endl;
				lines.push_back(cut_line);
				he_pair_idx.push_back(std::make_pair (idx1, idx2) );
			}
		}
	}
	// Find all the cut edges more than two segments. 
	// Find the first on edge point
	unsigned first_pos = size; 
	unsigned first_idx = 0; 
	unsigned second_idx = 0; 
	for (unsigned i = 0; i < pol.size(); ++i)
	{
		unsigned idx1, idx2 ;
		if (is_on_edge (orig_pol, pol[i], idx1) && !is_on_edge (orig_pol, pol[(i+1)%size], idx2))
		{
			first_pos = i ; 
			first_idx = idx1; break; 
		}
	}

	if (first_pos == size) return ;
	std::vector<Vector <Value> > cut_line ;
	cut_line.push_back(pol[first_pos]);
	std::cout<<"CutLine\nCL Point"<<pol[first_pos]<<std::endl;
	//cut_line.push_back(pol[(first_pos + 1)%size]);
	for (unsigned i = first_pos + 1 ; i < pol.size() + first_pos; ++i )
	{
		unsigned idx1, idx2 ;
		if (!is_on_edge (orig_pol, pol[i% size], idx1) && is_on_edge (orig_pol, pol[(i+1)%size], idx2))
		{
			cut_line.push_back(pol[(i)%size]);
			std::cout<<"CL Point"<<pol[(i)%size]<<std::endl;
			cut_line.push_back(pol[(i + 1)%size]);
			std::cout<<"CL Point"<<pol[(i + 1)%size]<<std::endl;
			second_idx = idx2 ;
			lines.push_back(cut_line);
			he_pair_idx.push_back(std::make_pair (first_idx, second_idx) );
			cut_line.clear();
		}
		else if (is_on_edge (orig_pol, pol[i % size], idx1) && !is_on_edge (orig_pol, pol[(i+1)%size], idx2)) 
		{
			std::cout<<"CutLine\n"; 
			cut_line.push_back(pol[i%size]);
			std::cout<<"CutLine\nCL Point"<<pol[i%size]<<std::endl;
			first_idx = idx1; 
		}
		else  if (!is_on_edge (orig_pol, pol[i % size], idx1) && !is_on_edge (orig_pol, pol[(i+1)%size], idx2)) 
		{
			cut_line.push_back(pol[(i) % size]);
			std::cout<<"CL Point"<<pol[(i)%size]<<std::endl;
		}
	}
}


/**
 * This function will return the cut lines. 
 */
template <typename Value>
bool intersect_polygon_polygon_coplanar (const Polygon <Value> & _pol1, const Polygon <Value> & _pol2, IntersectResult<Value> & _intr, 	std::vector<std::vector<Vector<Value> > > & cut_line, std::vector<std::vector<Vector<Value> > > & opposite_cut_line, std::vector<pair <unsigned, unsigned > > & hepair_idx , std::vector<pair <unsigned, unsigned > > & opposite_hepair_idx )
{
	std::cout<<"Pol1 "<<_pol1<<std::endl;
	std::cout<<"Pol2 "<<_pol2<<std::endl;

	Vector<Value> normal = _pol1.calculate_normal();
	unsigned x, y; 
    select_map_axis(normal, x, y);

	Polygon <Value> _pol1_2d, _pol2_2d;
	for (unsigned i = 0; i < _pol1.size(); ++i)
	{
		_pol1_2d.append_vertex (Vector<Value> (_pol1[i][x], _pol1[i][y]) );
	}

	for (unsigned i = 0; i < _pol2.size(); ++i)
	{
		_pol2_2d.append_vertex (Vector<Value> (_pol2[i][x], _pol2[i][y]) );
	}

	//Guess orientation
	int orient_pol1 = orient_2d (_pol1_2d[0], _pol1_2d[1], _pol1_2d[2]);
	int orient_pol2 = orient_2d (_pol2_2d[0], _pol2_2d[1], _pol2_2d[2]);

	// Two polygon are oriented oppositely.
	if (orient_pol1 * orient_pol2 < 0) return false; 

	unsigned size1 = _pol1_2d.size() ;
	unsigned size2 = _pol2_2d.size() ;
	if (orient_pol1 < 0)
	{
		//_reverse _pol1_2d and _pol2_2d
		for (unsigned i = 0; i <  size1 / 2; ++i)
		{
			swap (_pol1_2d[i], _pol1_2d[size1 - i - 1]) ;
		}

		for (unsigned i = 0; i < size2 /2 ; ++i)
		{
			swap (_pol2_2d[i], _pol2_2d[size1 - i - 1]) ;
		}		
	}

	//std::cout<<"POL1 2D "<<_pol1_2d<<std::endl;; 
	//std::cout<<"POL2 2D "<<_pol2_2d<<std::endl;; 

	IntersectResult<Value> inter_2d; 
	bool rvalue = intersect_conv_poly_poly_2d (_pol1_2d, _pol2_2d, inter_2d);

	if (rvalue == false) return false; 

//	std::vector<std::vector<Vector<Value> > > cut_line; 
	

	// compute the inverse table 
	std::vector<unsigned> table (size1);
	for (unsigned i = 0 ; i < size1 - 1 ; ++i)
	{
		table[i] = size1 - 2 - i ;
	}
	table[size1 - 1] = size1 - 1; 

	Polygon<Value> p = _pol1; 
	p.set_normal(p.calculate_normal());
	Plane<Value> plane = p.get_plane(); 

	//std::cout<<inter_2d.polygon<<std::endl;
    // Reverse mapping cut lines
	cut_lines (_pol1_2d, inter_2d.polygon, cut_line, hepair_idx);
	for (unsigned i = 0; i < cut_line.size(); ++i)
	{
		for (unsigned j = 0; j < cut_line[i].size(); ++j)
		{
			Vector<Value> point ;
			map_2D_point_to_3D_point(x, y, plane, cut_line[i][j], point ); 
			cut_line[i][j] = point;
		}
		if (orient_pol1 < Value(0) )
		{
			// Adjust orientation
			reverse (cut_line[i].begin(), cut_line[i].end());
/*
#ifdef _DEBUG
			if ( hepair_idx[i].first > 2 || hepair_idx[i].second > 2) throw ("Wrong IDX. ");
#endif
*/
			swap (hepair_idx[i].first, hepair_idx[i].second);
			hepair_idx[i].first =  table [hepair_idx[i].first ];
			hepair_idx[i].second = table [hepair_idx[i].second ];
		}
	}


	// reverse opposite cut lines
	cut_lines (_pol2_2d, inter_2d.polygon, opposite_cut_line, opposite_hepair_idx);
	for (unsigned i = 0; i < opposite_cut_line.size(); ++i)
	{
		for (unsigned j = 0; j < opposite_cut_line[i].size(); ++j)
		{
			Vector<Value> point ;
			map_2D_point_to_3D_point(x, y, plane, opposite_cut_line[i][j], point ); 
			opposite_cut_line[i][j] = point;
		}
		if (orient_pol1 < Value(0) )
		{
			// Adjust orientation
			reverse (opposite_cut_line[i].begin(), opposite_cut_line[i].end());
/*
#ifdef _DEBUG
			if ( opposite_hepair_idx[i].first > 2 || opposite_hepair_idx[i].second > 2) throw ("Wrong IDX. ");
#endif
*/
			swap (opposite_hepair_idx[i].first, opposite_hepair_idx[i].second);
			opposite_hepair_idx[i].first =  table [opposite_hepair_idx[i].first ];
			opposite_hepair_idx[i].second = table [opposite_hepair_idx[i].second ];
		}
	}

	// Reverse mapping intersection.
	switch (inter_2d.res_type)
	{
	case POINT: 
		_intr.res_type = POINT ;
		map_2D_point_to_3D_point(x, y, plane, inter_2d.point, _intr.point); 
		return true; 
		break;
	case SEGMENT :
		_intr.res_type = SEGMENT ;
		map_2D_point_to_3D_point(x, y, plane, inter_2d.segment.p0 , _intr.segment.p0); 
		map_2D_point_to_3D_point(x, y, plane, inter_2d.segment.p1 , _intr.segment.p1);
		return true; 
		break;
	case POLYGON :
		_intr.res_type = POLYGON ;
		if (orient_pol1 > 0)
		{
			for (unsigned i = 0; i < inter_2d.polygon.size(); ++i)
			{
				Vector<Value> p; 
				map_2D_point_to_3D_point(x, y, plane, inter_2d.polygon[i], p);
				_intr.polygon.append_vertex(p);
			}
		}
		else
		{
			for (int i = (int)inter_2d.polygon.size() - 1; i >=0 ; --i)
			{
				Vector<Value> p; 
				map_2D_point_to_3D_point(x, y, plane, inter_2d.polygon[i], p);
				_intr.polygon.append_vertex(p);
			}
		}
		return true; 
		break;
	default:
		return false;
	}
	return false; 
}


/**
 * This function now handle triangle intersects triangle.
 * We will improve it to handle polgon intersect polygon. 
 */
template <typename Value>
bool intersect_tri_tri (const Polygon <Value> & _pol1, const Polygon <Value> & _pol2, IntersectResult<Value> & _intr)
{
	Vector<Value> source(3), target(3); 
	int coplanar ;
	int rvalue =  tri_tri_intersection_test_3d(_pol1, _pol2, & coplanar, source, target);


	if (coplanar == 1) 
	{
		return intersect_polygon_polygon_coplanar(_pol1, _pol2, _intr);
	}

	switch (rvalue)
	{
	case 0: return false;
	case 1: 
		_intr.pos = INTERSECT ;
		_intr.res_type = SEGMENT ;
		_intr.segment = Segment<Value> (source, target) ;
		return true;

	case 2:
		_intr.pos = INTERSECT ;
		_intr.res_type = POINT ;
		_intr.point = source; 
		return true; 
	case 3:
		;
	}
	return rvalue; 

}

/*
template <typename Value>
bool predicate_polygon_polygon(const Polygon <Value> & _pol1 , const Polygon <Value> & _pol2)
{
	IntersectResult<Value> _intr; 
	return intersect(_pol1, _pol2, _intr);
}
*/

 /**
  * This function return a point inside of a polygon (convex or concave). The algorithm is as follows. 
Given a Simple_Polygon, find some point inside it. Here is a method based on the proof that there exists an internal diagonal, in [O'Rourke (C), 13-14]. The idea is that the midpoint of a diagonal is interior to the polygon.
Identify a convex vertex v; let its adjacent vertices be a and b.
For each other vertex q do:
If q is inside avb, compute distance to v (orthogonal to ab).
Save point q if distance is a new min.
If no point is inside, return centroid of avb.
Else if some point inside, qv is internal: return its midpoint.
	
  */
template <typename Value> 
Vector<Value> point_in_polygon (const Polygon <Value> & _pol1, Vector<Value> & normal)
{
	for (unsigned i = 0; i < _pol1.size(); ++i)
	{
		
	}
	return Vector<Value>(); 
}

#endif
